use std::vec;
use crate::json_parse::ECU_MAP;

pub struct UdsMsg<'a> {
    pub service_id: u8,
    pub nrc: u8,
    rsp_msg: &'a mut Vec<u8>,
}

impl UdsMsg<'_> {
    pub fn new<'a>(service_id: u8, rsp_msg: &mut Vec<u8>) -> UdsMsg {
        UdsMsg {
            service_id,
            nrc: 0,
            rsp_msg,
        }
    }

    pub fn create_nrc_msg(&mut self)
    {
        self.rsp_msg.clear();
        self.rsp_msg.push(0x7F);
        self.rsp_msg.push(self.service_id);
        self.rsp_msg.push(self.nrc);
    }

    pub fn create_rsp_msg(&mut self, data: &[u8])
    {
        self.rsp_msg.clear();
        self.rsp_msg.push(self.service_id + 0x40);
        if data.len() > 0
        {
            self.rsp_msg.extend_from_slice(data);
        }  
    }
}
pub fn parse_header(buf: &[u8]) -> (u16, u32)
{
    if buf.len() >= 8 {
        if buf[0] == 0x02 && buf[1] == 0xFD {
            let msg_type = ((buf[2] as u16) << 8) | buf[3] as u16;
            let payload_length = ((buf[4] as u32) << 24) | 
                                    ((buf[5] as u32) << 16) | 
                                    ((buf[6] as u32) << 8) | 
                                    buf[7] as u32;
            return (msg_type, payload_length);
        } else {
            println!("doip layer: doip header error. buf:{:X?}", buf);
            return (0, 0);
        }
    } else {
        return (0, 0);
    }
}

pub fn create_rsp_msg(loadtype: u16, payload: &[u8]) -> Vec<u8>
{
    let payload_len = payload.len();
    let mut buf = vec![];
    buf.push(0x02);
    buf.push(0xFD);
    buf.push((loadtype >> 8) as u8);
    buf.push(loadtype as u8);
    buf.push((payload_len >> 24) as u8);
    buf.push((payload_len >> 16) as u8);
    buf.push((payload_len >> 8) as u8);
    buf.push(payload_len as u8);
    buf.extend_from_slice(payload);
    return buf;
}

pub fn pack_payload(ta: u16, sa: u16, payload: &[u8]) -> Vec<u8>
{
    let mut buf = vec![];
    buf.push((sa >> 8) as u8);
    buf.push(sa as u8);
    buf.push((ta >> 8) as u8);
    buf.push(ta as u8);
    buf.extend_from_slice(payload);
    return buf;
}

pub fn function_address_frame_resp(loadtype: u16, ta: u16, usr_data: &[u8]) -> Vec<u8>
{
    let mut buf = vec![];
    let len = usr_data.len() + 4;
    let ecu_map = ECU_MAP.lock().unwrap();
    let ecu_map = ecu_map.get_ecumap();

    for (addr, _) in ecu_map.iter() {
        buf.push(0x02);
        buf.push(0xFD);
        buf.push((loadtype >> 8) as u8);
        buf.push(loadtype as u8);
        buf.push((len >> 24) as u8);
        buf.push((len >> 16) as u8);
        buf.push((len >> 8) as u8);
        buf.push(len as u8);
        buf.push((*addr >> 8) as u8);
        buf.push(*addr as u8);
        buf.push((ta >> 8) as u8);
        buf.push(ta as u8);
        buf.extend_from_slice(usr_data);
    }
    return buf;
}